/*
 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.box2dflash.dynamics {
	import org.box2dflash.common.math.*;
	import org.box2dflash.common.*;
	import org.box2dflash.collision.*;
	import org.box2dflash.collision.shapes.*;
	import org.box2dflash.dynamics.*;
	import org.box2dflash.dynamics.contacts.*;
	import org.box2dflash.dynamics.joints.*;
	use namespace Internal;

	public class World {
		// Construct a world object.
		/// @param worldAABB a bounding box that completely encompasses all your shapes.
		/// @param gravity the world gravity vector.
		/// @param doSleep improve performance by not simulating inactive bodies.
		public function World(worldAABB:AABB, gravity:Vec2, doSleep:Boolean) {
		
			m_destructionListener = null;
			m_boundaryListener = null;
			m_contactFilter = ContactFilter._defaultFilter;
			m_contactListener = null;
			m_debugDraw = null;
		
			m_bodyList = null;
			m_contactList = null;
			m_jointList = null;
		
			m_bodyCount = 0;
			m_contactCount = 0;
			m_jointCount = 0;
		
			m_positionCorrection = true;
			m_warmStarting = true;
			m_continuousPhysics = true;
		
			m_allowSleep = doSleep;
			m_gravity = gravity;
		
			m_lock = false;
		
			m_inv_dt0 = 0.0;
		
			m_contactManager.m_world = this;
			//void* mem = Alloc(sizeof(BroadPhase));
			m_broadPhase = new BroadPhase(worldAABB, m_contactManager);
		
			var bd:BodyDef = new BodyDef();
			m_groundBody = createBody(bd);
		}

		/// Destruct the world. All physics entities are destroyed and all heap memory is released.
		//~World();

		/// Register a destruction listener.
		public function set destructionListener(listener:DestructionListener):void {
			m_destructionListener = listener;
		}

		/// Register a broad-phase boundary listener.
		public function set boundaryListener(listener:BoundaryListener):void {
			m_boundaryListener = listener;
		}

		/// Register a contact filter to provide specific control over collision.
		/// Otherwise the default filter is used (_defaultFilter).
		public function set contactFilter(filter:ContactFilter):void {
			m_contactFilter = filter;
		}

		/// Register a contact event listener
		public function set contactListener(listener:ContactListener):void {
			m_contactListener = listener;
		}

		/// Register a routine for debug drawing. The debug draw functions are called
		/// inside the World::Step method, so make sure your renderer is ready to
		/// consume draw commands when you call Step().
		public function set debugDraw(debugDraw:DebugDraw):void {
			m_debugDraw = debugDraw;
		}

		/// Perform validation of internal data structures.
		public function validate():void {
			m_broadPhase.validate();
		}

		/// Get the number of broad-phase proxies.
		public function get proxyCount():int {
			return m_broadPhase.m_proxyCount;
		}

		/// Get the number of broad-phase pairs.
		public function get pairCount():int {
			return m_broadPhase.m_pairManager.m_pairCount;
		}

		/// Create a rigid body given a definition. No reference to the definition
		/// is retained.
		/// @warning This function is locked during callbacks.
		public function createBody(def:BodyDef):Body {
		
			//Settings.Assert(m_lock == false);
			if (m_lock == true) {
				return null;
			}
		
			//void* mem = m_blockAllocator.Allocate(sizeof(Body));
			var b:Body = new Body(def, this);
		
			// Add to world doubly linked list.
			b.m_prev = null;
			b.m_next = m_bodyList;
			if (m_bodyList) {
				m_bodyList.m_prev = b;
			}
			m_bodyList = b;
			++m_bodyCount;
		
			return b;
		}

		/// Destroy a rigid body given a definition. No reference to the definition
		/// is retained. This function is locked during callbacks.
		/// @warning This automatically deletes all associated shapes and joints.
		/// @warning This function is locked during callbacks.
		public function destroyBody(b:Body):void {
		
			//Settings.Assert(m_bodyCount > 0);
			//Settings.Assert(m_lock == false);
			if (m_lock == true) {
				return;
			}
		
			// Delete the attached joints.
			var jn:JointEdge = b.m_jointList;
			while (jn) {
				var jn0:JointEdge = jn;
				jn = jn.next;
			
				if (m_destructionListener) {
					m_destructionListener.sayGoodbyeJoint(jn0.joint);
				}
			
				destroyJoint(jn0.joint);
			}
		
			// Delete the attached shapes. This destroys broad-phase
			// proxies and pairs, leading to the destruction of contacts.
			var s:Shape = b.m_shapeList;
			while (s) {
				var s0:Shape = s;
				s = s.m_next;
			
				if (m_destructionListener) {
					m_destructionListener.sayGoodbyeShape(s0);
				}
			
				s0.destroyProxy(m_broadPhase);
				Shape.destroy(s0/*NOT USED, m_blockAllocator*/);
			}
		
			// Remove world body list.
			if (b.m_prev) {
				b.m_prev.m_next = b.m_next;
			}
		
			if (b.m_next) {
				b.m_next.m_prev = b.m_prev;
			}
		
			if (b == m_bodyList) {
				m_bodyList = b.m_next;
			}
		
			--m_bodyCount;
		//b->~Body();
		//m_blockAllocator.Free(b, sizeof(Body));
		}

		/// Create a joint to constrain bodies together. No reference to the definition
		/// is retained. This may cause the connected bodies to cease colliding.
		/// @warning This function is locked during callbacks.
		public function createJoint(def:JointDef):Joint {
		
			//Settings.Assert(m_lock == false);
			var j:Joint = Joint.Create(def/*NOT USED, m_blockAllocator*/);
		
			// Connect to the world list.
			j.m_prev = null;
			j.m_next = m_jointList;
			if (m_jointList) {
				m_jointList.m_prev = j;
			}
			m_jointList = j;
			++m_jointCount;
		
			// Connect to the bodies' doubly linked lists.
			j.m_node1.joint = j;
			j.m_node1.other = j.m_body2;
			j.m_node1.prev = null;
			j.m_node1.next = j.m_body1.m_jointList;
			if (j.m_body1.m_jointList) j.m_body1.m_jointList.prev = j.m_node1;
			j.m_body1.m_jointList = j.m_node1;
		
			j.m_node2.joint = j;
			j.m_node2.other = j.m_body1;
			j.m_node2.prev = null;
			j.m_node2.next = j.m_body2.m_jointList;
			if (j.m_body2.m_jointList) j.m_body2.m_jointList.prev = j.m_node2;
			j.m_body2.m_jointList = j.m_node2;
		
			// If the joint prevents collisions, then reset collision filtering.
			if (def.collideConnected == false) {
				// Reset the proxies on the body with the minimum number of shapes.
				var b:Body = def.body1.m_shapeCount < def.body2.m_shapeCount ? def.body1 : def.body2;
				for (var s:Shape = b.m_shapeList;s; s = s.m_next) {
					s.refilterProxy(m_broadPhase, b.m_xf);
				}
			}
		
			return j;
		}

		/// Destroy a joint. This may cause the connected bodies to begin colliding.
		/// @warning This function is locked during callbacks.
		public function destroyJoint(j:Joint):void {
		
			//Settings.Assert(m_lock == false);
			var collideConnected:Boolean = j.m_collideConnected;
		
			// Remove from the doubly linked list.
			if (j.m_prev) {
				j.m_prev.m_next = j.m_next;
			}
		
			if (j.m_next) {
				j.m_next.m_prev = j.m_prev;
			}
		
			if (j == m_jointList) {
				m_jointList = j.m_next;
			}
		
			// Disconnect from island graph.
			var body1:Body = j.m_body1;
			var body2:Body = j.m_body2;
		
			// Wake up connected bodies.
			body1.wakeUp();
			body2.wakeUp();
		
			// Remove from body 1.
			if (j.m_node1.prev) {
				j.m_node1.prev.next = j.m_node1.next;
			}
		
			if (j.m_node1.next) {
				j.m_node1.next.prev = j.m_node1.prev;
			}
		
			if (j.m_node1 == body1.m_jointList) {
				body1.m_jointList = j.m_node1.next;
			}
		
			j.m_node1.prev = null;
			j.m_node1.next = null;
		
			// Remove from body 2
			if (j.m_node2.prev) {
				j.m_node2.prev.next = j.m_node2.next;
			}
		
			if (j.m_node2.next) {
				j.m_node2.next.prev = j.m_node2.prev;
			}
		
			if (j.m_node2 == body2.m_jointList) {
				body2.m_jointList = j.m_node2.next;
			}
		
			j.m_node2.prev = null;
			j.m_node2.next = null;
		
			Joint.Destroy(j/*NOT USED, m_blockAllocator*/);
		
			//Settings.Assert(m_jointCount > 0);
			--m_jointCount;
		
			// If the joint prevents collisions, then reset collision filtering.
			if (collideConnected == false) {
				// Reset the proxies on the body with the minimum number of shapes.
				var b:Body = body1.m_shapeCount < body2.m_shapeCount ? body1 : body2;
				for (var s:Shape = b.m_shapeList;s; s = s.m_next) {
					s.refilterProxy(m_broadPhase, b.m_xf);
				}
			}
		}

		/// Re-filter a shape. This re-runs contact filtering on a shape.
		public function refilter(shape:Shape):void {
			shape.refilterProxy(m_broadPhase, shape.m_body.m_xf);
		}

		/// Enable/disable warm starting. For testing.
		public function set WarmStarting(flag:Boolean):void { 
			m_warmStarting = flag; 
		}

		/// Enable/disable position correction. For testing.
		public function set PositionCorrection(flag:Boolean):void { 
			m_positionCorrection = flag; 
		}

		/// Enable/disable continuous physics. For testing.
		public function set ContinuousPhysics(flag:Boolean):void { 
			m_continuousPhysics = flag; 
		}

		/// Get the number of bodies.
		public function get bodyCount():int {
			return m_bodyCount;
		}

		/// Get the number joints.
		public function get jointCount():int {
			return m_jointCount;
		}

		/// Get the number of contacts (each may have 0 or more contact points).
		public function get contactCount():int {
			return m_contactCount;
		}

		/// Change the global gravity vector.
		public function set Gravity(gravity:Vec2):void {
			m_gravity = gravity;
		}

		/// The world provides a single static ground body with no collision shapes.
		/// You can use this to simplify the creation of joints and static shapes.
		public function get groundBody():Body {
			return m_groundBody;
		}

		/// Take a time step. This performs collision detection, integration,
		/// and constraint solution.
		/// @param timeStep the amount of time to simulate, this should not vary.
		/// @param iterations the number of iterations to be used by the constraint solver.
		public function step(dt:Number, iterations:int):void {
		
			m_lock = true;
		
			var step:TimeStep = new TimeStep();
			step.dt = dt;
			step.maxIterations = iterations;
			if (dt > 0.0) {
				step.inv_dt = 1.0 / dt;
			}
		else {
				step.inv_dt = 0.0;
			}
		
			step.dtRatio = m_inv_dt0 * dt;
		
			step.positionCorrection = m_positionCorrection;
			step.warmStarting = m_warmStarting;
		
			// Update contacts.
			m_contactManager.collide();
		
			// Integrate velocities, solve velocity constraints, and integrate positions.
			if (step.dt > 0.0) {
				solve(step);
			}
		
			// Handle TOI events.
			if (m_continuousPhysics && step.dt > 0.0) {
				solveTOI(step);
			}
		
			// Draw debug information.
			drawDebugData();
		
			m_inv_dt0 = step.inv_dt;
			m_lock = false;
		}

		/// Query the world for all shapes that potentially overlap the
		/// provided AABB. You provide a shape pointer buffer of specified
		/// size. The number of shapes found is returned.
		/// @param aabb the query box.
		/// @param shapes a user allocated shape pointer array of size maxCount (or greater).
		/// @param maxCount the capacity of the shapes array.
		/// @return the number of shapes found in aabb.
		public function query(aabb:AABB, shapes:Array, maxCount:int):int {
		
			//void** results = (void**)m_stackAllocator.Allocate(maxCount * sizeof(void*));
			var results:Array = new Array(maxCount);
		
			var count:int = m_broadPhase.queryAABB(aabb, results, maxCount);
		
			for (var i:int = 0;i < count; ++i) {
				shapes[i] = results[i];
			}
		
			//m_stackAllocator.Free(results);
			return count;
		}

		/// Get the world body list. With the returned body, use Body::GetNext to get
		/// the next body in the world list. A NULL body indicates the end of the list.
		/// @return the head of the world body list.
		public function get bodyList():Body {
			return m_bodyList;
		}

		/// Get the world joint list. With the returned joint, use Joint::GetNext to get
		/// the next joint in the world list. A NULL joint indicates the end of the list.
		/// @return the head of the world joint list.
		public function get jointList():Joint {
			return m_jointList;
		}

		//--------------- Internals Below -------------------
		// Internal yet public to make life easier.

		// Find islands, integrate and solve constraints, solve position constraints
		Internal function solve(step:TimeStep):void {
		
			var b:Body;
		
			m_positionIterationCount = 0;
		
			// Size the island for the worst case.
			var island:Island = new Island(m_bodyCount, m_contactCount, m_jointCount, m_stackAllocator, m_contactListener);
		
			// Clear all the island flags.
			for (b = m_bodyList;b; b = b.m_next) {
				b.m_flags &= ~Body.e_islandFlag;
			}
			for (var c:Contact = m_contactList;c; c = c.m_next) {
				c.m_flags &= ~Contact.e_islandFlag;
			}
			for (var j:Joint = m_jointList;j; j = j.m_next) {
				j.m_islandFlag = false;
			}
		
			// Build and simulate all awake islands.
			var stackSize:int = m_bodyCount;
			//Body** stack = (Body**)m_stackAllocator.Allocate(stackSize * sizeof(Body*));
			var stack:Array = new Array(stackSize);
			for (var seed:Body = m_bodyList;seed; seed = seed.m_next) {
				if (seed.m_flags & (Body.e_islandFlag | Body.e_sleepFlag | Body.e_frozenFlag)) {
					continue;
				}
			
				if (seed.isStatic) {
					continue;
				}
			
				// Reset island and stack.
				island.clear();
				var stackCount:int = 0;
				stack[stackCount++] = seed;
				seed.m_flags |= Body.e_islandFlag;
			
				// Perform a depth first search (DFS) on the constraint graph.
				while (stackCount > 0) {
					// Grab the next body off the stack and add it to the island.
					b = stack[--stackCount];
					island.addBody(b);
				
					// Make sure the body is awake.
					b.m_flags &= ~Body.e_sleepFlag;
				
					// To keep islands as small as possible, we don't
					// propagate islands across static bodies.
					if (b.isStatic) {
						continue;
					}
				
					var other:Body;
					// Search all contacts connected to this body.
					for (var cn:ContactEdge = b.m_contactList;cn; cn = cn.next) {
						// Has this contact already been added to an island?
						if (cn.contact.m_flags & (Contact.e_islandFlag | Contact.e_nonSolidFlag)) {
							continue;
						}
					
						// Is this contact touching?
						if (cn.contact.m_manifoldCount == 0) {
							continue;
						}
					
						island.addContact(cn.contact);
						cn.contact.m_flags |= Contact.e_islandFlag;
					
						//var other:Body = cn.other;
						other = cn.other;
					
						// Was the other body already added to this island?
						if (other.m_flags & Body.e_islandFlag) {
							continue;
						}
					
						//Settings.Assert(stackCount < stackSize);
						stack[stackCount++] = other;
						other.m_flags |= Body.e_islandFlag;
					}
				
					// Search all joints connect to this body.
					for (var jn:JointEdge = b.m_jointList;jn; jn = jn.next) {
						if (jn.joint.m_islandFlag == true) {
							continue;
						}
					
						island.addJoint(jn.joint);
						jn.joint.m_islandFlag = true;
					
						//var other:Body = jn.other;
						other = jn.other;
						if (other.m_flags & Body.e_islandFlag) {
							continue;
						}
					
						//Settings.Assert(stackCount < stackSize);
						stack[stackCount++] = other;
						other.m_flags |= Body.e_islandFlag;
					}
				}
			
				island.solve(step, m_gravity, m_positionCorrection, m_allowSleep);
				//m_positionIterationCount = Math.max(m_positionIterationCount, island.m_positionIterationCount);
				if (island.m_positionIterationCount > m_positionIterationCount) {
					m_positionIterationCount = island.m_positionIterationCount;
				}
			
				// Post solve cleanup.
				for (var i:int = 0;i < island.m_bodyCount; ++i) {
					// Allow static bodies to participate in other islands.
					b = island.m_bodies[i];
					if (b.isStatic) {
						b.m_flags &= ~Body.e_islandFlag;
					}
				}
			}
		
			//m_stackAllocator.Free(stack);
		
			// Synchronize shapes, check for out of range bodies.
			for (b = m_bodyList;b; b = b.m_next) {
				if (b.m_flags & (Body.e_sleepFlag | Body.e_frozenFlag)) {
					continue;
				}
			
				if (b.isStatic) {
					continue;
				}
			
				// Update shapes (for broad-phase). If the shapes go out of
				// the world AABB then shapes and contacts may be destroyed,
				// including contacts that are
				var inRange:Boolean = b.synchronizeShapes();
			
				// Did the body's shapes leave the world?
				if (inRange == false && m_boundaryListener != null) {
					m_boundaryListener.violation(b);
				}
			}
		
			// Commit shape proxy movements to the broad-phase so that new contacts are created.
			// Also, some contacts can be destroyed.
			m_broadPhase.commit();
		}

		// Find TOI contacts and solve them.
		Internal function solveTOI(step:TimeStep):void {
		
			var b:Body;
			var s1:Shape;
			var s2:Shape;
			var b1:Body;
			var b2:Body;
			var cn:ContactEdge;
		
			// Reserve an island and a stack for TOI island solution.
			var island:Island = new Island(m_bodyCount, Settings._maxTOIContactsPerIsland, 0, m_stackAllocator, m_contactListener);
			var stackSize:int = m_bodyCount;
		
			//Body** stack = (Body**)m_stackAllocator.Allocate(stackSize * sizeof(Body*));
			var stack:Array = new Array(stackSize);
		
			for (b = m_bodyList;b; b = b.m_next) {
				b.m_flags &= ~Body.e_islandFlag;
				b.m_sweep.t0 = 0.0;
			}
		
			var c:Contact;
			for (c = m_contactList;c; c = c.m_next) {
				// Invalidate TOI
				c.m_flags &= ~(Contact.e_toiFlag | Contact.e_islandFlag);
			}
		
			// Find TOI events and solve them.
			for (;;) {
				// Find the first TOI.
				var minContact:Contact = null;
				var minTOI:Number = 1.0;
			
				for (c = m_contactList;c; c = c.m_next) {
					if (c.m_flags & (Contact.e_slowFlag | Contact.e_nonSolidFlag)) {
						continue;
					}
				
					// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
					var toi:Number = 1.0;
					if (c.m_flags & Contact.e_toiFlag) {
						// This contact has a valid cached TOI.
						toi = c.m_toi;
					}
				else {
						// Compute the TOI for this contact.
						s1 = c.m_shape1;
						s2 = c.m_shape2;
						b1 = s1.m_body;
						b2 = s2.m_body;
					
						if ((b1.isStatic || b1.isSleeping) && (b2.isStatic || b2.isSleeping)) {
							continue;
						}
					
						// Put the sweeps onto the same time interval.
						var t0:Number = b1.m_sweep.t0;
					
						if (b1.m_sweep.t0 < b2.m_sweep.t0) {
							t0 = b2.m_sweep.t0;
							b1.m_sweep.advance(t0);
						}
					else if (b2.m_sweep.t0 < b1.m_sweep.t0) {
							t0 = b1.m_sweep.t0;
							b2.m_sweep.advance(t0);
						}
					
						//Settings.Assert(t0 < 1.0f);
					
						// Compute the time of impact.
						toi = TimeOfImpact.timeOfImpact(c.m_shape1, b1.m_sweep, c.m_shape2, b2.m_sweep);
						//Settings.Assert(0.0 <= toi && toi <= 1.0);
						if (toi > 0.0 && toi < 1.0) {
							//toi = Math.min((1.0 - toi) * t0 + toi, 1.0);
							toi = (1.0 - toi) * t0 + toi;
							if (toi > 1) toi = 1;
						}
					
					
						c.m_toi = toi;
						c.m_flags |= Contact.e_toiFlag;
					}
				
					if (Number.MIN_VALUE < toi && toi < minTOI) {
						// This is the minimum TOI found so far.
						minContact = c;
						minTOI = toi;
					}
				}
			
				if (minContact == null || 1.0 - 100.0 * Number.MIN_VALUE < minTOI) {
					// No more TOI events. Done!
					break;
				}
			
				// Advance the bodies to the TOI.
				s1 = minContact.m_shape1;
				s2 = minContact.m_shape2;
				b1 = s1.m_body;
				b2 = s2.m_body;
				b1.advance(minTOI);
				b2.advance(minTOI);
			
				// The TOI contact likely has some new contact points.
				minContact.update(m_contactListener);
				minContact.m_flags &= ~Contact.e_toiFlag;
			
				if (minContact.m_manifoldCount == 0) {
					// This shouldn't happen. Numerical error?
					//Assert(false);
					continue;
				}
			
				// Build the TOI island. We need a dynamic seed.
				var seed:Body = b1;
				if (seed.isStatic) {
					seed = b2;
				}
			
				// Reset island and stack.
				island.clear();
				var stackCount:int = 0;
				stack[stackCount++] = seed;
				seed.m_flags |= Body.e_islandFlag;
			
				// Perform a depth first search (DFS) on the contact graph.
				while (stackCount > 0) {
					// Grab the next body off the stack and add it to the island.
					b = stack[--stackCount];
					island.addBody(b);
				
					// Make sure the body is awake.
					b.m_flags &= ~Body.e_sleepFlag;
				
					// To keep islands as small as possible, we don't
					// propagate islands across static bodies.
					if (b.isStatic) {
						continue;
					}
				
					// Search all contacts connected to this body.
					for (cn = b.m_contactList;cn; cn = cn.next) {
						// Does the TOI island still have space for contacts?
						if (island.m_contactCount == island.m_contactCapacity) {
							continue;
						}
					
						// Has this contact already been added to an island? Skip slow or non-solid contacts.
						if (cn.contact.m_flags & (Contact.e_islandFlag | Contact.e_slowFlag | Contact.e_nonSolidFlag)) {
							continue;
						}
					
						// Is this contact touching? For performance we are not updating this contact.
						if (cn.contact.m_manifoldCount == 0) {
							continue;
						}
					
						island.addContact(cn.contact);
						cn.contact.m_flags |= Contact.e_islandFlag;
					
						// Update other body.
						var other:Body = cn.other;
					
						// Was the other body already added to this island?
						if (other.m_flags & Body.e_islandFlag) {
							continue;
						}
					
						// March forward, this can do no harm since this is the min TOI.
						if (other.isStatic == false) {
							other.advance(minTOI);
							other.wakeUp();
						}
					
						//Settings.Assert(stackCount < stackSize);
						stack[stackCount++] = other;
						other.m_flags |= Body.e_islandFlag;
					}
				}
			
				var subStep:TimeStep = new TimeStep();
				subStep.dt = (1.0 - minTOI) * step.dt;
				//Settings.Assert(subStep.dt > Number.MIN_VALUE);
				subStep.inv_dt = 1.0 / subStep.dt;
				subStep.maxIterations = step.maxIterations;
			
				island.solveTOI(subStep);
			
				var i:int;
				// Post solve cleanup.
				for (i = 0;i < island.m_bodyCount; ++i) {
					// Allow bodies to participate in future TOI islands.
					b = island.m_bodies[i];
					b.m_flags &= ~Body.e_islandFlag;
				
					if (b.m_flags & (Body.e_sleepFlag | Body.e_frozenFlag)) {
						continue;
					}
				
					if (b.isStatic) {
						continue;
					}
				
					// Update shapes (for broad-phase). If the shapes go out of
					// the world AABB then shapes and contacts may be destroyed,
					// including contacts that are
					var inRange:Boolean = b.synchronizeShapes();
				
					// Did the body's shapes leave the world?
					if (inRange == false && m_boundaryListener != null) {
						m_boundaryListener.violation(b);
					}
				
					// Invalidate all contact TOIs associated with this body. Some of these
					// may not be in the island because they were not touching.
					for (cn = b.m_contactList;cn; cn = cn.next) {
						cn.contact.m_flags &= ~Contact.e_toiFlag;
					}
				}
			
				for (i = 0;i < island.m_contactCount; ++i) {
					// Allow contacts to participate in future TOI islands.
					c = island.m_contacts[i];
					c.m_flags &= ~(Contact.e_toiFlag | Contact.e_islandFlag);
				}
			
				// Commit shape proxy movements to the broad-phase so that new contacts are created.
				// Also, some contacts can be destroyed.
				m_broadPhase.commit();
			}
		
		//m_stackAllocator.Free(stack);
		}

		static private var s_jointColor:Color = new Color(0.5, 0.8, 0.8);

		//
		Internal function drawJoint(joint:Joint):void {
		
			var b1:Body = joint.m_body1;
			var b2:Body = joint.m_body2;
			var xf1:XForm = b1.m_xf;
			var xf2:XForm = b2.m_xf;
			var x1:Vec2 = xf1.position;
			var x2:Vec2 = xf2.position;
			var p1:Vec2 = joint.anchor1;
			var p2:Vec2 = joint.anchor2;
		
			//Color color(0.5f, 0.8f, 0.8f);
			var color:Color = s_jointColor;
		
			switch (joint.m_type) {
				case Joint.e_distanceJoint:
					m_debugDraw.drawSegment(p1, p2, color);
					break;
		
				case Joint.e_pulleyJoint:
					{
				var pulley:PulleyJoint = (joint as PulleyJoint);
				var s1:Vec2 = pulley.groundAnchor1;
				var s2:Vec2 = pulley.groundAnchor2;
				m_debugDraw.drawSegment(s1, p1, color);
				m_debugDraw.drawSegment(s2, p2, color);
				m_debugDraw.drawSegment(s1, s2, color);
			}
					break;
		
		case Joint.e_mouseJoint:
			m_debugDraw.drawSegment(p1, p2, color);
			break;
		
		default:
			if (b1 != m_groundBody)
				m_debugDraw.drawSegment(x1, p1, color);
			m_debugDraw.drawSegment(p1, p2, color);
			if ( b2 != m_groundBody)
				m_debugDraw.drawSegment(x2, p2, color);
			}
		}
	
	static private var s_coreColor:Color = new Color(0.9, 0.6, 0.6);
	Internal function drawShape(shape:Shape, xf:XForm, color:Color, core:Boolean) : void{
		
		var coreColor:Color = s_coreColor;
		
		switch (shape.m_type)
		{
		case Shape.e_circleShape:
			{
				var circle:CircleShape = (shape as CircleShape);
				
				var center:Vec2 = Box2dMath.mulX(xf, circle.m_localPosition);
				var radius:Number = circle.m_radius;
				var axis:Vec2 = xf.R.col1;
				
				m_debugDraw.drawSolidCircle(center, radius, axis, color);
				
				if (core)
				{
					m_debugDraw.drawCircle(center, radius - Settings._toiSlop, coreColor);
				}
			}
			break;
		
		case Shape.e_polygonShape:
			{
				var i:int;
				var poly:PolygonShape = (shape as PolygonShape);
				var vertexCount:int = poly.vertexCount;
				var localVertices:Array = poly.vertices;
				
				//Assert(vertexCount <= _maxPolygonVertices);
				var vertices:Array = new Array(Settings._maxPolygonVertices);
				
				for (i = 0; i < vertexCount; ++i)
				{
					vertices[i] = Box2dMath.mulX(xf, localVertices[i]);
				}
				
				m_debugDraw.drawSolidPolygon(vertices, vertexCount, color);
				
				if (core)
				{
					var localCoreVertices:Array = poly.coreVertices;
					for (i = 0; i < vertexCount; ++i)
					{
						vertices[i] = Box2dMath.mulX(xf, localCoreVertices[i]);
					}
					m_debugDraw.drawPolygon(vertices, vertexCount, coreColor);
				}
			}
			break;
		}
	}
	
	
	static private var s_xf:XForm = new XForm();
	Internal function drawDebugData() : void{
		
		if (m_debugDraw == null)
		{
			return;
		}
		
		m_debugDraw.m_sprite.graphics.clear();
		
		var flags:uint = m_debugDraw.flags;
		
		var i:int;
		var b:Body;
		var s:Shape;
		var j:Joint;
		var bp:BroadPhase;
		var invQ:Vec2 = new Vec2;
		var x1:Vec2 = new Vec2;
		var x2:Vec2 = new Vec2;
		var color:Color = new Color(0,0,0);
		var xf:XForm;
		var b1:AABB = new AABB();
		var b2:AABB = new AABB();
		var vs:Array = [new Vec2(), new Vec2(), new Vec2(), new Vec2()];
		
		if (flags & DebugDraw.e_shapeBit)
		{
			var core:Boolean = (flags & DebugDraw.e_coreShapeBit) == DebugDraw.e_coreShapeBit;
			
			for (b = m_bodyList; b; b = b.m_next)
			{
				xf = b.m_xf;
				for (s = b.shapeList; s; s = s.m_next)
				{
					if (b.isStatic)
					{
						drawShape(s, xf, new Color(0.5, 0.9, 0.5), core);
					}
					else if (b.isSleeping)
					{
						drawShape(s, xf, new Color(0.5, 0.5, 0.9), core);
					}
					else
					{
						drawShape(s, xf, new Color(0.9, 0.9, 0.9), core);
					}
				}
			}
		}
		
		if (flags & DebugDraw.e_jointBit)
		{
			for (j = m_jointList; j; j = j.m_next)
			{
				//if (j.m_type != Joint.e_mouseJoint)
				//{
					drawJoint(j);
				//}
			}
		}
		
		if (flags & DebugDraw.e_pairBit)
		{
			bp = m_broadPhase;
			//Vec2 invQ;
			invQ = new Vec2(1.0 / bp.m_quantizationFactor.x, 1.0 / bp.m_quantizationFactor.y);
			//Color color(0.9f, 0.9f, 0.3f);
			color = new Color(0.9, 0.9, 0.3);
			
			for (i = 0; i < Pair._tableCapacity; ++i)
			{
				var index:uint = bp.m_pairManager.m_hashTable[i];
				while (index != Pair._nullPair)
				{
					var pair:Pair = bp.m_pairManager.m_pairs[ index ];
					var p1:Proxy = bp.m_proxyPool[ pair.proxyId1 ];
					var p2:Proxy = bp.m_proxyPool[ pair.proxyId2 ];
					
					//AABB b1, ;
					b1.lowerBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p1.lowerBounds[0]].value;
					b1.lowerBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p1.lowerBounds[1]].value;
					b1.upperBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p1.upperBounds[0]].value;
					b1.upperBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p1.upperBounds[1]].value;
					b2.lowerBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p2.lowerBounds[0]].value;
					b2.lowerBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p2.lowerBounds[1]].value;
					b2.upperBound.x = bp.m_worldAABB.lowerBound.x + invQ.x * bp.m_bounds[0][p2.upperBounds[0]].value;
					b2.upperBound.y = bp.m_worldAABB.lowerBound.y + invQ.y * bp.m_bounds[1][p2.upperBounds[1]].value;
					
					//Vec2 x1 = 0.5f * (b1.lowerBound + b1.upperBound);
					x1.x = 0.5 * (b1.lowerBound.x + b1.upperBound.x);
					x1.y = 0.5 * (b1.lowerBound.y + b1.upperBound.y);
					//Vec2 x2 = 0.5f * (.lowerBound + b2.upperBound);
					x2.x = 0.5 * (b2.lowerBound.x + b2.upperBound.x);
					x2.y = 0.5 * (b2.lowerBound.y + b2.upperBound.y);
					
					m_debugDraw.drawSegment(x1, x2, color);
					
					index = pair.next;
				}
			}
		}
		
		if (flags & DebugDraw.e_aabbBit)
		{
			bp = m_broadPhase;
			var worldLower:Vec2 = bp.m_worldAABB.lowerBound;
			var worldUpper:Vec2 = bp.m_worldAABB.upperBound;
			
			//Vec2 invQ;
			invQ = new Vec2(1.0 / bp.m_quantizationFactor.x, 1.0 / bp.m_quantizationFactor.y);
			//Color color(0.9f, 0.3f, 0.9f);
			color = new Color(0.9, 0.3, 0.9);
			for (i = 0; i < Settings._maxProxies; ++i)
			{
				var p:Proxy = bp.m_proxyPool[ i];
				if (p.isValid == false)
				{
					continue;
				}
				
				//AABB b1;
				b1.lowerBound.x = worldLower.x + invQ.x * bp.m_bounds[0][p.lowerBounds[0]].value;
				b1.lowerBound.y = worldLower.y + invQ.y * bp.m_bounds[1][p.lowerBounds[1]].value;
				b1.upperBound.x = worldLower.x + invQ.x * bp.m_bounds[0][p.upperBounds[0]].value;
				b1.upperBound.y = worldLower.y + invQ.y * bp.m_bounds[1][p.upperBounds[1]].value;
				
				//Vec2 vs[4];
				vs[0].Set(b1.lowerBound.x, b1.lowerBound.y);
				vs[1].Set(b1.upperBound.x, b1.lowerBound.y);
				vs[2].Set(b1.upperBound.x, b1.upperBound.y);
				vs[3].Set(b1.lowerBound.x, b1.upperBound.y);
				
				m_debugDraw.drawPolygon(vs, 4, color);
			}
			
			//Vec2 vs[4];
			vs[0].Set(worldLower.x, worldLower.y);
			vs[1].Set(worldUpper.x, worldLower.y);
			vs[2].Set(worldUpper.x, worldUpper.y);
			vs[3].Set(worldLower.x, worldUpper.y);
			m_debugDraw.drawPolygon(vs, 4, new Color(0.3, 0.9, 0.9));
		}
		
		if (flags & DebugDraw.e_obbBit)
		{
			//Color color(0.5f, 0.3f, 0.5f);
			color = new Color(0.5, 0.3, 0.5);
			
			for (b = m_bodyList; b; b = b.m_next)
			{
				xf = b.m_xf;
				for (s = b.shapeList; s; s = s.m_next)
				{
					if (s.m_type != Shape.e_polygonShape)
					{
						continue;
					}
					
					var poly:PolygonShape = (s as PolygonShape);
					var obb:OBB = poly.oBB;
					var h:Vec2 = obb.extents;
					//Vec2 vs[4];
					vs[0].Set(-h.x, -h.y);
					vs[1].Set( h.x, -h.y);
					vs[2].Set( h.x,  h.y);
					vs[3].Set(-h.x,  h.y);
					
					for (i = 0; i < 4; ++i)
					{
						//vs[i] = obb.center + Mul(obb.R, vs[i]);
						var tMat:Mat22 = obb.R;
						var tVec:Vec2 = vs[i];
						var tX:Number;
						tX      = obb.center.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
						vs[i].y = obb.center.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
						vs[i].x = tX;
						//vs[i] = Mul(xf, vs[i]);
						tMat = xf.R;
						tX      = xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
						vs[i].y = xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
						vs[i].x = tX;
					}
					
					m_debugDraw.drawPolygon(vs, 4, color);
				}
			}
		}
		
		if (flags & DebugDraw.e_centerOfMassBit)
		{
			for (b = m_bodyList; b; b = b.m_next)
			{
				xf = s_xf;
				xf.R = b.m_xf.R;
				xf.position = b.worldCenter;
				m_debugDraw.drawXForm(xf);
			}
		}
	}

	Internal var m_blockAllocator:*;
	Internal var m_stackAllocator:*;

	Internal var m_lock:Boolean;

	Internal var m_broadPhase:BroadPhase;
	Internal var m_contactManager:ContactManager = new ContactManager();

	Internal var m_bodyList:Body;
	Internal var m_jointList:Joint;

	// Do not access
	Internal var m_contactList:Contact;

	Internal var m_bodyCount:int;
	Internal var m_contactCount:int;
	Internal var m_jointCount:int;

	Internal var m_gravity:Vec2;
	Internal var m_allowSleep:Boolean;

	Internal var m_groundBody:Body;

	Internal var m_destructionListener:DestructionListener;
	Internal var m_boundaryListener:BoundaryListener;
	Internal var m_contactFilter:ContactFilter;
	Internal var m_contactListener:ContactListener;
	Internal var m_debugDraw:DebugDraw;

	Internal var m_inv_dt0:Number;

	Internal var m_positionIterationCount:int;

	// This is for debugging the solver.
	static Internal var m_positionCorrection:Boolean;

	// This is for debugging the solver.
	static Internal var m_warmStarting:Boolean;

	// This is for debugging the solver.
	static Internal var m_continuousPhysics:Boolean;
	}
}
